﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Runtime.InteropServices;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using Emgu.CV;
using Emgu.Util;
using Emgu.CV.Structure;
using Emgu.CV.CvEnum;


namespace Autofocus
{
    public partial class Form1 : Form
    {
        MotiCam My_MotiCam;
        IntPtr[] hCameraList;
        Stepper My_Stepper;
        Int32 X_sml = 4;
        Int32 X_med = 8;
        Int32 X_lrg = 80;
        Int32 Y_sml = 1;
        Int32 Y_med = 2;
        Int32 Y_lrg = 80;
        Int32 Z_sml = 40;
        Int32 Z_med = 80;
        Int32 Z_lrg = 400;

        #region Camera UI Controls
        private void UpdateControls(MotiCam camera)
        {
            comboBox_Binning.Items.Clear();
            for (int i = 0; i < camera.binCount; i++)
            {
                String BinString = camera.m_pWidth[i].ToString() + " x " + camera.m_pHeight[i].ToString();
                comboBox_Binning.Items.Add(BinString);
                comboBox_Binning.SelectedIndex = 0;
            }

            numericUpDown_Exposure.Minimum = Convert.ToDecimal(camera.m_fMinExposure);
            numericUpDown_Exposure.Maximum = Convert.ToDecimal(camera.m_fMaxExposure);
            numericUpDown_Exposure.Value = Convert.ToDecimal(camera.m_fExposure);

            trackBar_Offset.Minimum = camera.m_iMinOffset;
            trackBar_Offset.Maximum = camera.m_iMaxOffset;
            trackBar_Offset.Value = camera.m_iOffset;
            textBox_Offset.Text = camera.m_iOffset.ToString();

            trackBar_Red_Gain.Minimum = 0;
            trackBar_Red_Gain.Maximum = camera.m_iGainCount - 1;
            trackBar_Red_Gain.Value = camera.m_RedGainIndex;
            textBox_Red_Gain.Text = camera.m_pGainList[camera.m_RedGainIndex].ToString();

            trackBar_Green_Gain.Minimum = 0;
            trackBar_Green_Gain.Maximum = camera.m_iGainCount - 1;
            trackBar_Green_Gain.Value = camera.m_GreenGainIndex;
            textBox_Green_Gain.Text = camera.m_pGainList[camera.m_GreenGainIndex].ToString();

            trackBar_Blue_Gain.Minimum = 0;
            trackBar_Blue_Gain.Maximum = camera.m_iGainCount - 1;
            trackBar_Blue_Gain.Value = camera.m_BlueGainIndex;
            textBox_Blue_Gain.Text = camera.m_pGainList[camera.m_BlueGainIndex].ToString();
        }

        private void comboBox_Cameras_SelectedIndexChanged(object sender, EventArgs e)
        {
            IntPtr hCameraHandle;
            hCameraHandle = hCameraList[comboBox_Cameras.SelectedIndex];
            if (My_MotiCam != null)
            {
                My_MotiCam.CloseCamera();
            }
            My_MotiCam = new MotiCam(hCameraHandle);
            if (My_MotiCam.OpenCamera())
            {
                UpdateControls(My_MotiCam);

            }
            else
                MessageBox.Show("Unable to open camera");

        }

        private void trackBar_Offset_ValueChanged(object sender, EventArgs e)
        {
            My_MotiCam.m_iOffset = trackBar_Offset.Value;
            My_MotiCam.setRGBOffset();
            UpdateControls(My_MotiCam);
        }

        private void trackBar_Red_Gain_ValueChanged(object sender, EventArgs e)
        {
            My_MotiCam.m_RedGainIndex = trackBar_Red_Gain.Value;
            My_MotiCam.setRGBGainIndex();
            UpdateControls(My_MotiCam);
        }

        private void trackBar_Green_Gain_ValueChanged(object sender, EventArgs e)
        {
            My_MotiCam.m_GreenGainIndex = trackBar_Green_Gain.Value;
            My_MotiCam.setRGBGainIndex();
            UpdateControls(My_MotiCam);
        }

        private void trackBar_Blue_Gain_ValueChanged(object sender, EventArgs e)
        {
            My_MotiCam.m_BlueGainIndex = trackBar_Blue_Gain.Value;
            My_MotiCam.setRGBGainIndex();
            UpdateControls(My_MotiCam);
        }

        private void numericUpDown_Exposure_ValueChanged(object sender, EventArgs e)
        {
            My_MotiCam.m_fExposure = Convert.ToSingle(numericUpDown_Exposure.Value);
            My_MotiCam.setExposure();
        }

        private void btn_Get_Single_Frame_Click(object sender, EventArgs e)
        {
            Image<Bgr, Byte> img = GetImage(My_MotiCam, 0);
            imageBox1.Image = img;
        }

        private void btn_Live_Cam_Click(object sender, EventArgs e)
        {
            if (timer1.Enabled)
                timer1.Enabled = false;
            else
                timer1.Enabled = true;
        }

        private void timer1_Tick(object sender, EventArgs e)
        {
            Image<Bgr, Byte> img = GetImage(My_MotiCam, 0);

            imageBox1.Image = img;
        }

        #endregion Camera UI Controls

        #region Motor Controls

        private void btn_X_fwd_sml_Click(object sender, EventArgs e)
        {
            int n = My_Stepper.MoveX(X_sml);

            if (n == 0)
            {
                if (checkBox_Update_Image.Checked)
                {
                    System.Threading.Thread.Sleep(2000);
                    Image<Bgr, Byte> img = GetImage(My_MotiCam, 0);
                    imageBox1.Image = img;
                }         
            }
            else if (n == 1)
            {
                MessageBox.Show("Timed out waiting for motor. Is it connected?");
            }
            else if (n == 2)
            {
                MessageBox.Show("Limit switch hit! Turn off power and manually correct...");
            }
        }

        private void btn_X_fwd_med_Click(object sender, EventArgs e)
        {
            int n = My_Stepper.MoveX(X_med);

            if (n == 0)
            {
                if (checkBox_Update_Image.Checked)
                {
                    System.Threading.Thread.Sleep(2000);
                    Image<Bgr, Byte> img = GetImage(My_MotiCam, 0);
                    imageBox1.Image = img;
                }
            }
            else if (n == 1)
            {
                MessageBox.Show("Timed out waiting for motor. Is it connected?");
            }
            else if (n == 2)
            {
                MessageBox.Show("Limit switch hit! Turn off power and manually correct...");
            }
        }

        private void btn_X_fwd_lrg_Click(object sender, EventArgs e)
        {
            int n = My_Stepper.MoveX(X_lrg);

            if (n == 0)
            {
                if (checkBox_Update_Image.Checked)
                {
                    System.Threading.Thread.Sleep(2000);
                    Image<Bgr, Byte> img = GetImage(My_MotiCam, 0);
                    imageBox1.Image = img;
                }
            }
            else if (n == 1)
            {
                MessageBox.Show("Timed out waiting for motor. Is it connected?");
            }
            else if (n == 2)
            {
                MessageBox.Show("Limit switch hit! Turn off power and manually correct...");
            }
        }

        private void btn_X_back_sml_Click(object sender, EventArgs e)
        {
            int n = My_Stepper.MoveX(-X_sml);

            if (n == 0)
            {
                if (checkBox_Update_Image.Checked)
                {
                    System.Threading.Thread.Sleep(2000);
                    Image<Bgr, Byte> img = GetImage(My_MotiCam, 0);
                    imageBox1.Image = img;
                }
            }
            else if (n == 1)
            {
                MessageBox.Show("Timed out waiting for motor. Is it connected?");
            }
            else if (n == 2)
            {
                MessageBox.Show("Limit switch hit! Turn off power and manually correct...");
            }
        }

        private void btn_X_back_med_Click(object sender, EventArgs e)
        {
            int n = My_Stepper.MoveX(-X_med);

            if (n == 0)
            {
                if (checkBox_Update_Image.Checked)
                {
                    System.Threading.Thread.Sleep(2000);
                    Image<Bgr, Byte> img = GetImage(My_MotiCam, 0);
                    imageBox1.Image = img;
                }
            }
            else if (n == 1)
            {
                MessageBox.Show("Timed out waiting for motor. Is it connected?");
            }
            else if (n == 2)
            {
                MessageBox.Show("Limit switch hit! Turn off power and manually correct...");
            }
        }

        private void btn_X_back_lrg_Click(object sender, EventArgs e)
        {
            int n = My_Stepper.MoveX(-X_lrg);

            if (n == 0)
            {
                if (checkBox_Update_Image.Checked)
                {
                    System.Threading.Thread.Sleep(2000);
                    Image<Bgr, Byte> img = GetImage(My_MotiCam, 0);
                    imageBox1.Image = img;
                }
            }
            else if (n == 1)
            {
                MessageBox.Show("Timed out waiting for motor. Is it connected?");
            }
            else if (n == 2)
            {
                MessageBox.Show("Limit switch hit! Turn off power and manually correct...");
            }
        }

        private void btn_Y_fwd_sml_Click(object sender, EventArgs e)
        {
            int n = My_Stepper.MoveY(Y_sml);

            if (n == 0)
            {
                if (checkBox_Update_Image.Checked)
                {
                    System.Threading.Thread.Sleep(2000);
                    Image<Bgr, Byte> img = GetImage(My_MotiCam, 0);
                    imageBox1.Image = img;
                }
            }
            else if (n == 1)
            {
                MessageBox.Show("Timed out waiting for motor. Is it connected?");
            }
            else if (n == 2)
            {
                MessageBox.Show("Limit switch hit! Turn off power and manually correct...");
            }
        }

        private void btn_Y_fwd_med_Click(object sender, EventArgs e)
        {
            int n = My_Stepper.MoveY(Y_med);

            if (n == 0)
            {
                if (checkBox_Update_Image.Checked)
                {
                    System.Threading.Thread.Sleep(2000);
                    Image<Bgr, Byte> img = GetImage(My_MotiCam, 0);
                    imageBox1.Image = img;
                }
            }
            else if (n == 1)
            {
                MessageBox.Show("Timed out waiting for motor. Is it connected?");
            }
            else if (n == 2)
            {
                MessageBox.Show("Limit switch hit! Turn off power and manually correct...");
            }
        }

        private void btn_Y_fwd_lrg_Click(object sender, EventArgs e)
        {
            int n = My_Stepper.MoveY(Y_lrg);

            if (n == 0)
            {
                if (checkBox_Update_Image.Checked)
                {
                    System.Threading.Thread.Sleep(2000);
                    Image<Bgr, Byte> img = GetImage(My_MotiCam, 0);
                    imageBox1.Image = img;
                }
            }
            else if (n == 1)
            {
                MessageBox.Show("Timed out waiting for motor. Is it connected?");
            }
            else if (n == 2)
            {
                MessageBox.Show("Limit switch hit! Turn off power and manually correct...");
            }
        }

        private void btn_Y_back_sml_Click(object sender, EventArgs e)
        {
            int n = My_Stepper.MoveY(-Y_sml);

            if (n == 0)
            {
                if (checkBox_Update_Image.Checked)
                {
                    System.Threading.Thread.Sleep(2000);
                    Image<Bgr, Byte> img = GetImage(My_MotiCam, 0);
                    imageBox1.Image = img;
                }
            }
            else if (n == 1)
            {
                MessageBox.Show("Timed out waiting for motor. Is it connected?");
            }
            else if (n == 2)
            {
                MessageBox.Show("Limit switch hit! Turn off power and manually correct...");
            }
        }

        private void btn_Y_back_med_Click(object sender, EventArgs e)
        {
            int n = My_Stepper.MoveY(-Y_med);

            if (n == 0)
            {
                if (checkBox_Update_Image.Checked)
                {
                    System.Threading.Thread.Sleep(2000);
                    Image<Bgr, Byte> img = GetImage(My_MotiCam, 0);
                    imageBox1.Image = img;
                }
            }
            else if (n == 1)
            {
                MessageBox.Show("Timed out waiting for motor. Is it connected?");
            }
            else if (n == 2)
            {
                MessageBox.Show("Limit switch hit! Turn off power and manually correct...");
            }
        }

        private void btn_Y_back_lrg_Click(object sender, EventArgs e)
        {
            int n = My_Stepper.MoveY(-Y_lrg);

            if (n == 0)
            {
                if (checkBox_Update_Image.Checked)
                {
                    System.Threading.Thread.Sleep(2000);
                    Image<Bgr, Byte> img = GetImage(My_MotiCam, 0);
                    imageBox1.Image = img;
                }
            }
            else if (n == 1)
            {
                MessageBox.Show("Timed out waiting for motor. Is it connected?");
            }
            else if (n == 2)
            {
                MessageBox.Show("Limit switch hit! Turn off power and manually correct...");
            }
        }

        private void btn_Z_fwd_sml_Click(object sender, EventArgs e)
        {
            int n = My_Stepper.MoveZ(Z_sml);

            if (n == 0)
            {
                if (checkBox_Update_Image.Checked)
                {
                    System.Threading.Thread.Sleep(1000);
                    Image<Bgr, Byte> img = GetImage(My_MotiCam, 0);
                    imageBox1.Image = img;
                }
            }
            else if (n == 1)
            {
                MessageBox.Show("Timed out waiting for motor. Is it connected?");
            }
            else if (n == 2)
            {
                MessageBox.Show("Limit switch hit! Turn off power and manually correct...");
            }
        }

        private void btn_Z_fwd_med_Click(object sender, EventArgs e)
        {
            int n = My_Stepper.MoveZ(Z_med);

            if (n == 0)
            {
                if (checkBox_Update_Image.Checked)
                {
                    System.Threading.Thread.Sleep(1000);
                    Image<Bgr, Byte> img = GetImage(My_MotiCam, 0);
                    imageBox1.Image = img;
                }
            }
            else if (n == 1)
            {
                MessageBox.Show("Timed out waiting for motor. Is it connected?");
            }
            else if (n == 2)
            {
                MessageBox.Show("Limit switch hit! Turn off power and manually correct...");
            }
        }

        private void btn_Z_fwd_lrg_Click(object sender, EventArgs e)
        {
            int n = My_Stepper.MoveZ(Z_lrg);

            if (n == 0)
            {
                if (checkBox_Update_Image.Checked)
                {
                    System.Threading.Thread.Sleep(1000);
                    Image<Bgr, Byte> img = GetImage(My_MotiCam, 0);
                    imageBox1.Image = img;
                }
            }
            else if (n == 1)
            {
                MessageBox.Show("Timed out waiting for motor. Is it connected?");
            }
            else if (n == 2)
            {
                MessageBox.Show("Limit switch hit! Turn off power and manually correct...");
            }
        }

        private void btn_Z_back_sml_Click(object sender, EventArgs e)
        {
            int n = My_Stepper.MoveZ(-Z_sml);

            if (n == 0)
            {
                if (checkBox_Update_Image.Checked)
                {
                    System.Threading.Thread.Sleep(1000);
                    Image<Bgr, Byte> img = GetImage(My_MotiCam, 0);
                    imageBox1.Image = img;
                }
            }
            else if (n == 1)
            {
                MessageBox.Show("Timed out waiting for motor. Is it connected?");
            }
            else if (n == 2)
            {
                MessageBox.Show("Limit switch hit! Turn off power and manually correct...");
            }
        }

        private void btn_Z_back_med_Click(object sender, EventArgs e)
        {
            int n = My_Stepper.MoveZ(-Z_med);

            if (n == 0)
            {
                if (checkBox_Update_Image.Checked)
                {
                    System.Threading.Thread.Sleep(1000);
                    Image<Bgr, Byte> img = GetImage(My_MotiCam, 0);
                    imageBox1.Image = img;
                }
            }
            else if (n == 1)
            {
                MessageBox.Show("Timed out waiting for motor. Is it connected?");
            }
            else if (n == 2)
            {
                MessageBox.Show("Limit switch hit! Turn off power and manually correct...");
            }
        }

        private void btn_Z_back_lrg_Click(object sender, EventArgs e)
        {
            int n = My_Stepper.MoveZ(-Z_lrg);

            if (n == 0)
            {
                if (checkBox_Update_Image.Checked)
                {
                    System.Threading.Thread.Sleep(1000);
                    Image<Bgr, Byte> img = GetImage(My_MotiCam, 0);
                    imageBox1.Image = img;
                }
            }
            else if (n == 1)
            {
                MessageBox.Show("Timed out waiting for motor. Is it connected?");
            }
            else if (n == 2)
            {
                MessageBox.Show("Limit switch hit! Turn off power and manually correct...");
            }
        }


        #endregion

        #region Image Proc Functions

        Image<Bgr, Byte> GetImage(MotiCam camera, Int32 binningIndex)
        {
            if (camera != null)
            {
                Byte[] buffer = new Byte[camera.m_pWidth[binningIndex] * camera.m_pHeight[binningIndex] * camera.m_bitDepth * camera.m_colorChannel / 8];
                camera.GetFrame(ref buffer);
                Image<Bgr, Byte> img = new Image<Bgr, Byte>(camera.m_pWidth[binningIndex], camera.m_pHeight[binningIndex]);

                GCHandle handle = GCHandle.Alloc(buffer, GCHandleType.Pinned);
                IntPtr imageHeaderForBytes = Marshal.AllocHGlobal(Marshal.SizeOf(typeof(MIplImage)));
                CvInvoke.cvInitImageHeader(imageHeaderForBytes, new System.Drawing.Size(camera.m_pWidth[binningIndex], camera.m_pHeight[binningIndex]), Image<Bgr, Byte>.CvDepth, camera.m_colorChannel, 0, 4); // align
                Marshal.WriteIntPtr(imageHeaderForBytes, (int)Marshal.OffsetOf(typeof(MIplImage), "imageData"), handle.AddrOfPinnedObject());
                CvInvoke.cvCopy(imageHeaderForBytes, img, IntPtr.Zero);

                Marshal.FreeHGlobal(imageHeaderForBytes);
                handle.Free();
                return img;
            }

            Image<Bgr, Byte> temp = new Image<Bgr, byte>(400, 200, new Bgr(255, 0, 0)); //Create an image of 400x200 of Blue color
            MCvFont f = new MCvFont(FONT.CV_FONT_HERSHEY_COMPLEX, 1.0, 1.0); //Create the font

            temp.Draw("No camera connected!", ref f, new System.Drawing.Point(10, 80), new Bgr(0, 255, 0)); //Draw message on the image using the specific font
            return temp;
        }      
        
        #endregion Image Proc Functions

        public Form1()
        {
            InitializeComponent();
        }

        private void Form1_Load(object sender, EventArgs e)
        {
            // Find Cameras and populate combo-box
            IntPtr hCameraHandle;
            hCameraList = new IntPtr[10];
            Int32 indx = 0;
            try
            {
                hCameraHandle = Motic.MUCam_findCamera();
                while (hCameraHandle != IntPtr.Zero)
                {
                    hCameraList[indx++] = hCameraHandle;
                    comboBox_Cameras.Items.Add(Motic.GetType(hCameraHandle));
                    hCameraHandle = Motic.MUCam_findCamera();
                }
                if (comboBox_Cameras.Items.Count > 0)
                {
                    comboBox_Cameras.SelectedIndex = 0;
                }
            }
            catch
            {
                MessageBox.Show("Unable to find any connected cameras!");
            }

            // Connect to Stepper Motor
            My_Stepper = new Stepper();
            My_Stepper.Connect(textBox_SP_name.Text, 9600);

        }

        private Boolean Autofocus()
        { 
            Int32 stops = Convert.ToInt32(nud_afc_Stops.Value); 
            Int32 step = Convert.ToInt32(nud_afc_StepSz.Value);
            Image<Bgr, Byte>[] img_array = new Image<Bgr,byte>[stops];

            Double[] std_dev_array = new Double[stops];
            double thresh, threshLinking;
            thresh =  Convert.ToDouble(nud_Thresh.Value);
            threshLinking = Convert.ToDouble(nud_ThreshLinking.Value);
            textBox_Variances.Clear();

            // Get series of images and find the variance of the canny image for each
            My_Stepper.MoveZ((step * stops * -1) / 2);

            System.Threading.Thread.Sleep(1500);
            for (int i = 0; i < stops; i++)
            {
                img_array[i] = GetImage(My_MotiCam, 0);
                imageBox1.Image = img_array[i];
                imageBox1.Refresh();
                //img_array[i].Save("C:\\Users\\Ming\\Pictures\\Micrographs\\MechStage\\AFC\\AFC_Test_" + String.Format("{0:000}", i) + ".tiff");

                imageBox2.Image = Calculate_Variance(img_array[i], thresh, threshLinking, ref std_dev_array[i]);
                imageBox2.Refresh();
                textBox_Variances.Text += "Variance of image " + i.ToString() + ": " + String.Format("{0:00.0000}", std_dev_array[i]) + "\r\n";
                textBox_Variances.Refresh();

                // If focus has got worse two steps in a row
                if (i > 1)
                {
                    if ((std_dev_array[i] < std_dev_array[i - 1]) && (std_dev_array[i - 1] < std_dev_array[i - 2]))
                    {
                        My_Stepper.MoveZ(-2 * step);
                        // Take the in focus picture
                        System.Threading.Thread.Sleep(1500);
                        img_array[0] = GetImage(My_MotiCam, 0);
                        imageBox1.Image = img_array[0];
                        return true;
                    }
                }

                // Wait for camera to acquire a new buffer and move to new position
                My_Stepper.MoveZ(step);
                System.Threading.Thread.Sleep(1500);
            }
            return false;
        }

        private void Capture_Sequence(Int32 num_images,  String file_name)
        {
            Int32 counter = 0;
            Int32 cum_y_error = 0;
            Int32 cum_x_error = 0;
            Int32 delta_X = 0, delta_Y = 0;

            Int32 min_delta_X = Convert.ToInt32(nud_min_delta_X.Value);
            Int32 X_Step = Convert.ToInt32(nud_X_motor_step_size.Value);
            Int32 X_Max_Steps = 9;

            Int32 Y_Step = Convert.ToInt32(nud_Y_motor_step_size.Value);

            Int32 Y_Pos_high_limit = Convert.ToInt32(nud_Y_pos_high_limit.Value);
            Int32 Y_Pos_low_limit = Convert.ToInt32(nud_Y_pos_low_limit.Value);
            double Metal_Resin_threshold = Convert.ToDouble(nud_hist_threshold.Value);

            Image<Bgr, Byte> img_1;
            Image<Bgr, Byte> img_2; 
           

            for (int j = 0; j < num_images; j++)
            {
                // Update the UI to show current action
                label_current_image.Text = "Capturing image " + (j+1).ToString() + " of " + num_images.ToString();
                label_current_image.Refresh();

                // Take a picture
                System.Threading.Thread.Sleep(2000);
                img_1 = GetImage(My_MotiCam, 0);
                imageBox2.Image = img_1;
                imageBox2.Refresh();
                img_1.Save(file_name + "\\img_" + String.Format("{0:000}", (j + Convert.ToInt32(nud_file_start_index.Value))) + ".tiff");

                // Move in the X direction
                while (Math.Abs(delta_X) < Math.Abs(min_delta_X))
                {
                    if (counter > X_Max_Steps)
                    {
                        MessageBox.Show("Tried to move 10 times in X direction can could not find shift. Aborting...");
                        return;
                    }

                    // Move in X direction a certain amount
                    label_current_X_action.Text = "Moving in X direction attempt " + counter.ToString() + " of " + X_Max_Steps.ToString();
                    label_current_X_action.Refresh();
                    My_Stepper.MoveX(X_Step);
                    System.Threading.Thread.Sleep(2000);

                    // Take a new picture
                    img_2 = GetImage(My_MotiCam, 0);
                    imageBox1.Image = img_2;
                    imageBox1.Refresh();

                    // Obtain position correlation
                    Phase.Calc_Phase_Correlation(img_1.Convert<Gray, float>(), img_2.Convert<Gray, float>(), ref delta_X, ref delta_Y);
                    label_phase_corr.Text = "Delta_X: " + delta_X.ToString() + " Delta_Y: " + delta_Y.ToString() + " Cumulative_X: " + cum_x_error.ToString() + " Cumulative_Y: " + cum_y_error.ToString();
                    label_phase_corr.Refresh();
                    counter++;
                }

                delta_X = 0;
                delta_Y = 0;
                counter = 0;
                if (!(Adjust_Y_Position(Metal_Resin_threshold, Y_Pos_high_limit, Y_Pos_low_limit, Y_Step, 15)))
                {
                    MessageBox.Show("Tried to adjust Y position 15 times but could not center coating. Aborting...");
                    return;
                }

                // Every fifth step or if image focus is below AFC threshold then autofocus 
                Image<Bgr, Byte> current_image = new Image<Bgr, Byte>(imageBox1.Image.Bitmap);
                Double variance = 0;
                Calculate_Variance(current_image, Convert.ToDouble(nud_Thresh.Value), Convert.ToDouble(nud_ThreshLinking.Value), ref variance);
                label_current_variance.Text = "Variance of current image: " + variance.ToString();
                label_current_variance.Refresh();
                if (((j % 5) == 0) || (variance < Convert.ToDouble(nud_afc_threshold.Value))) 
                {
                    if (!Autofocus())
                    {
                        MessageBox.Show("Unable to autofocus. Aborting...");
                        return;
                    }
                }

            }
            MessageBox.Show("Finished capturing image sequence.");
                
        }






       

        private Boolean Adjust_Y_Position(double threshold, Int32 Y_pos_high_limit, Int32 Y_pos_low_limit, Int32 Y_Step, Int32 count)
        {

            for (int i = 0; i < count; i++)
            {
                Image<Bgr, Byte> current_image = GetImage(My_MotiCam, 0);
                imageBox1.Image = current_image;
                imageBox1.Refresh();
                double avg_val = Get_Avg_Val(current_image, threshold);
                label_histogram.Text = "Average of gray image: " + avg_val.ToString();
                label_histogram.Refresh();

                if (avg_val < Y_pos_low_limit)
                {
                    label_current_Y_action.Text = "Adjusting in Y direction, attempt " + i.ToString() + " of " + count.ToString();
                    label_current_Y_action.Refresh();                    
                    My_Stepper.MoveY(Y_Step);
                    System.Threading.Thread.Sleep(2000);
                }
                else if (avg_val > Y_pos_high_limit)
                {
                    label_current_Y_action.Text = "Adjusting in Y direction, attempt " + i.ToString() + " of " + count.ToString();
                    label_current_Y_action.Refresh(); 
                    My_Stepper.MoveY(-Y_Step);
                    System.Threading.Thread.Sleep(2000);
                }
                else
                {
                    // Position is ok
                    //MessageBox.Show("Successfully centred the coating!");
                    break;
                }
            }

            Image<Bgr, Byte> updated_image = GetImage(My_MotiCam, 0);
            double check_avg_val = Get_Avg_Val(updated_image, threshold);
            if (check_avg_val < Y_pos_low_limit || check_avg_val > Y_pos_high_limit)
            {
                // Coating is still not centered so indicate an error and abort
                return false;
            }
            else
            {
                label_current_Y_action.Text = "Y Position OK";
                label_current_Y_action.Refresh(); 
                return true;
            }
        }

        // This function returns the average value of the image after binary thresholding at the given threshold
        private Double Get_Avg_Val(Image<Bgr, Byte> current_image, double threshold)
        { 
            
            Image<Gray, float> Gray_current_image = current_image.Convert<Gray, float>();
            Gray hist_threshold = new Gray(threshold);
            Gray max_value = new Gray(255);
            Gray_current_image._ThresholdBinary(hist_threshold, max_value);
            //imageBox2.Image = Gray_current_image; 
            Gray My_Gray;
            MCvScalar My_MCvScalar;
            Gray_current_image.AvgSdv(out My_Gray, out My_MCvScalar);
            return My_Gray.Intensity;        
        }

        // Calculate and display the histogram for the current image to aid in threshold selection
        // Then display average value after thresholding at set threshold
        private void button_Calc_Histogram_Click(object sender, EventArgs e)
        {
            
            Image<Bgr, Byte> current_image = new Image<Bgr,byte>(imageBox1.Image.Bitmap);
            Image<Gray, float> Gray_current_image = current_image.Convert<Gray, float>();
            histogramBox1.ClearHistogram();
            histogramBox1.GenerateHistograms(Gray_current_image, 255);
            histogramBox1.Refresh();

            double threshold = Convert.ToDouble(nud_hist_threshold.Value);
            double avg_val = Get_Avg_Val(current_image, threshold);
            label_histogram.Text = "Average of gray image: " + avg_val.ToString();
            label_histogram.Refresh();
        }

        private void button_Autofocus_Click(object sender, EventArgs e)
        {
            Autofocus();
        }

        private void btn_cpy_across_Click(object sender, EventArgs e)
        {
            imageBox2.Image = imageBox1.Image;
        }

        private void btn_Phase_Corr_Click(object sender, EventArgs e)
        {
            // Current image
            Image<Bgr, Byte> Image_1 = new Image<Bgr, byte>(imageBox1.Image.Bitmap);
            // Previous image
            Image<Bgr, Byte> Image_2 = new Image<Bgr, byte>(imageBox2.Image.Bitmap);

            Image<Gray, float> img_1 = Image_1.Convert<Gray, float>();
            Image<Gray, float> img_2 = Image_2.Convert<Gray, float>();

            imageBox1.Image = img_1;
            imageBox2.Image = img_2;

            Int32 delta_X = 0, delta_Y = 0;
            Phase.Calc_Phase_Correlation(img_1, img_2, ref delta_X, ref delta_Y);

            label_phase_corr.Text = "Delta_X: " + delta_X.ToString() + " Delta_Y: " + delta_Y.ToString();
        }

        private void btn_Cap_Seq_Click(object sender, EventArgs e)
        {
            Capture_Sequence(Convert.ToInt32(nud_no_images.Value), textBox_file_name.Text);
        }


        private void button2_Click(object sender, EventArgs e)
        {
            double thresh, threshLinking;
            thresh = Convert.ToDouble(nud_Thresh.Value);
            threshLinking = Convert.ToDouble(nud_ThreshLinking.Value);

            Image<Bgr, Byte>img_1 = new Image<Bgr, Byte>(imageBox1.Image.Bitmap);
            Double variance = 0;
            Image<Gray, Byte> img_2 = Calculate_Variance(img_1, thresh, threshLinking, ref variance);
            imageBox2.Image = img_2;
            
            label_current_variance.Text = "Variance of current image: " + String.Format("{0:00.0000}", variance);
        }

        private Image<Gray, Byte> Calculate_Variance(Image<Bgr, Byte> img_1, Double thresh, Double threshLinking, ref Double variance)
        {      
            Image<Gray, Byte> My_Processed_Image = img_1.Convert<Gray, Byte>().PyrDown().PyrUp().Canny(new Gray(thresh), new Gray(threshLinking));

            Gray My_Gray;
            MCvScalar My_MCvScalar;
            My_Processed_Image.AvgSdv(out My_Gray, out My_MCvScalar);
            Double std_dev = My_MCvScalar.v0;
            variance = (std_dev * std_dev);
            return My_Processed_Image;        
        }



    }
}
